#include "Shape.h"
#include <math.h>
#include "GaussJordan.h"


Plane::Plane( Point3 widthVector, double width,
              Point3 lengthVector, double length,
              Point3 center)
{
    Point3 foundNormal;
    foundNormal.x = widthVector.y *lengthVector.z -  widthVector.z * lengthVector.y;
    foundNormal.y = widthVector.z *lengthVector.x -  widthVector.x * lengthVector.z;
    foundNormal.z = widthVector.x *lengthVector.y -  widthVector.y * lengthVector.x;
    
    //normalize
    double magnitude = sqrt( dotPoints(foundNormal, foundNormal) );
    scalePoint( foundNormal, 1/magnitude, &m_normal );
    
    m_width = widthVector;
    m_length = lengthVector;
    m_maxWidth = width/2;
    m_maxLength = length/2;

    m_center = center;
    m_planeD = dotPoints( m_normal, center );

}

void Plane::getIntercept( Point3 startPoint,
                          Point3 direction,
                          double * interceptTime,
                          Point3 * interceptPoint,
                          Point3 * interceptNormal )
{
    double dotWithPlane = dotPoints ( m_normal, direction) ;

    if ( abs( dotWithPlane ) < ZERO_APPROX )
    {
        *interceptTime = NO_INTERCEPT;
        return;
    }

    Point3 difference;
    subtractPoints( startPoint, m_center, &difference );

    *interceptTime = - dotPoints( m_normal, difference ) 
                        / dotWithPlane ;

    if ( *interceptTime < 0 || *interceptTime > 1 )
    {
        *interceptTime = NO_INTERCEPT;
        return;
    }

    if ( dotWithPlane < 0 )
    {
        scalePoint( m_normal, 1, interceptNormal);
    }
    else
    {
        scalePoint( m_normal, -1, interceptNormal);
    }


    //Find point
    Point3 scaledPt;
    scalePoint( direction, *interceptTime, &scaledPt );
    addPoints( startPoint, scaledPt, interceptPoint );

    //check intercept point in bounds

    double matrix[3][4];
    char lost = 2;

    if ( m_width.x == 0 && m_length.x == 0 )
    {
        lost = 0;
    }
    else if ( m_width.y == 0 && m_length.y == 0 )
    {
        lost = 1;
    }

    if ( lost == 0 )
    {
        matrix[0][0] = m_width.y;
        matrix[0][1] = m_length.y;
        matrix[0][2] = interceptPoint->y - m_center.y;

        matrix[1][0] = m_width.z;
        matrix[1][1] = m_length.z;
        matrix[1][2] = interceptPoint->z - m_center.z;
    }
    else if ( lost == 1 )
    {
        matrix[0][0] = m_width.x;
        matrix[0][1] = m_length.x;
        matrix[0][2] = interceptPoint->x - m_center.x;

        matrix[1][0] = m_width.z;
        matrix[1][1] = m_length.z;
        matrix[1][2] = interceptPoint->z - m_center.z;
    }
    else
    {
        matrix[0][0] = m_width.x;
        matrix[0][1] = m_length.x;
        matrix[0][2] = interceptPoint->x - m_center.x;

        matrix[1][0] = m_width.y;
        matrix[1][1] = m_length.y;
        matrix[1][2] = interceptPoint->y - m_center.y;
    }

    double results[3];
    gauss_jordan(2, matrix, results);
    if ( results[0] > m_maxWidth || results[0] < -m_maxWidth
         || results[1] > m_maxLength || results[1] < -m_maxLength )
    {
        //out of bounds
        *interceptTime = NO_INTERCEPT;
        return;
    }

}

Sphere::Sphere( Point3 center, double radius )
{
    m_radius = radius;
    m_center = center;
}

void Sphere::getIntercept( Point3 startPoint,
                          Point3 direction,
                          double * interceptTime,
                          Point3 * interceptPoint,
                          Point3 * interceptNormal )
{

    double xComponent = startPoint.x - m_center.x;
    double yComponent = startPoint.y - m_center.y;
    double zComponent = startPoint.z - m_center.z;

    //Quadratic
    double a;
    double b;
    double c;

    a = pow( direction.x, 2) + pow( direction.y, 2) + pow( direction.z, 2);
    b = -2 * ( xComponent*direction.x + yComponent*direction.y + zComponent*direction.z );
    c = pow( xComponent, 2) + pow( yComponent, 2) + pow( zComponent, 2) - m_radius;

    double deter = pow(b, 2) - 4 * a * c;
    if ( deter < ZERO_APPROX )
    {
        *interceptTime = NO_INTERCEPT;
        return;
    }

    double t1;
    double t2;
    //t1 = (-b - sqrt( deter )) / (2*a);
    //t2 = (-b + sqrt( deter )) / (2*a);

    t2 = - (-b - sqrt( deter )) / (2*a);
    t1 = - (-b + sqrt( deter )) / (2*a);

    if ( t2 < ZERO_APPROX || t1 > 1 )
    {
        *interceptTime = NO_INTERCEPT;
        return;
    }

    if ( t1 < ZERO_APPROX )
    {
        //inside circle
        *interceptTime = t2;
    }
    else
    {
        *interceptTime = t1;
    }


    Point3 scaledPt;
    scalePoint( direction, *interceptTime, &scaledPt );
    addPoints ( startPoint, scaledPt, interceptPoint );

    Point3 foundNormal;
    if ( *interceptTime == t2 )
    {
        //inside
        subtractPoints( m_center, *interceptPoint, interceptNormal );
    }
    else
    {
        subtractPoints( *interceptPoint, m_center, interceptNormal );
    }
    //normalize
    //double magnitude = sqrt( dotPoints(foundNormal, foundNormal) );
    normalize( interceptNormal );    
}